#include "foregroundextractor.h"


ForegroundExtractor::ForegroundExtractor(DISPLAY_TYPE displayType,
                                         DELTA_MODE deltaMode,
                                         POST_TRAITEMENT postTraitement,
                                         int referenceSize,
                                         int runningAverageSize,
                                         int movementHistorySize,
                                         int movementDensitySize,
                                         int movementDensityThreshold,
                                         int resultThreshold,
                                         bool greyscale) :
    _pos(0),
    _displayType(displayType),
    _deltaMode(deltaMode),
    _postTraitement(postTraitement),
    _referenceSize(referenceSize),
    _runningAverageSize(runningAverageSize),
    _movementHistorySize(movementHistorySize),
    _movementDensitySize(movementDensitySize),
    _movementDensityThreshold(movementDensityThreshold),
    _resultThreshold(resultThreshold),
    _greyscale(greyscale),
    _blur(false),
    _threshold(false),
    _rows(480),
    _cols(640)
{
    assert((runningAverageSize > 0) && (runningAverageSize <= 255));
    _capture.open(0);
    _capture.set(CV_CAP_PROP_FRAME_WIDTH, _cols);
    _capture.set(CV_CAP_PROP_FRAME_HEIGHT, _rows);
    _movementHistoryImage.create(_rows,_cols,CV_32SC1);
    _movementDensityImage.create(_rows,_cols,CV_32SC1);
    _difference.create(_rows,_cols,CV_8UC1);
    _result.create(_rows,_cols,CV_8UC1);
    if(_greyscale)
    {
        _source.create(_rows,_cols,CV_8UC1);
        _reference.create(_rows,_cols,CV_8UC1);
        _runningAverage.create(_rows,_cols,CV_16UC1);
    }
    else
    {
        _source.create(_rows,_cols,CV_8UC3);
        _reference.create(_rows,_cols,CV_8UC3);
        _runningAverage.create(_rows,_cols,CV_16UC3);
    }
}

ForegroundExtractor::~ForegroundExtractor()
{
    _capture.release();
}

int ForegroundExtractor::cols() const
{
    return _cols;
}

int ForegroundExtractor::rows() const
{
    return _rows;
}

void ForegroundExtractor::updateDelta(Mat &frame)
{
    Mat  reference;
    switch (_deltaMode) {
    case DELTA_MODE_PREVIOUS_FRAME:
        reference = _source;
        break;
    case DELTA_MODE_LEARNED_FRAME:
        reference = _reference;
        break;
    case DELTA_MODE_AVERAGE:
        frame.copyTo(reference);
        runningAverage(reference);
        break;
    }
    //Create output if necessary
    if(!((_difference.type() == CV_8UC1)
         && (_difference.cols == frame.cols)
         && (_difference.rows == frame.rows)))
        _difference.create(frame.rows,frame.cols,CV_8UC1);
    if(_greyscale)
        for(int y = 0; y < reference.rows; y++)
            for(int x = 0; x < reference.cols; x++)
                _difference.at<uchar>(y,x) = saturate_cast<uchar>(abs(reference.at<uchar>(y,x) - frame.at<uchar>(y,x)));
    else
        for(int y = 0; y < reference.rows; y++)
            for(int x = 0; x < reference.cols; x++) {
                _difference.at<uchar>(y,x) = saturate_cast<uchar>(abs(
                                                                      ((reference.at<Vec3b>(y,x)[0] + reference.at<Vec3b>(y,x)[1] + reference.at<Vec3b>(y,x)[2]))
                        - ((frame.at<Vec3b>(y,x)[0] + frame.at<Vec3b>(y,x)[1] + frame.at<Vec3b>(y,x)[2]))
                        )/3);
            }
}

void ForegroundExtractor::updateRunningAverage(Mat &frame)
{
    if(!_greyscale)
    {
        for(int y = 0; y < frame.rows; y++) {
            for(int x = 0; x < frame.cols; x++) {
                if(_pos > _runningAverageSize) {
                    _runningAverage.at<Vec3w>(y,x)[0] -= _runningAverage.at<Vec3w>(y,x)[0] / _runningAverageSize;
                    _runningAverage.at<Vec3w>(y,x)[1] -= _runningAverage.at<Vec3w>(y,x)[1] / _runningAverageSize;
                    _runningAverage.at<Vec3w>(y,x)[2] -= _runningAverage.at<Vec3w>(y,x)[2] / _runningAverageSize;
                }
                _runningAverage.at<Vec3w>(y,x)[0] += frame.at<Vec3b>(y,x)[0];
                _runningAverage.at<Vec3w>(y,x)[1] += frame.at<Vec3b>(y,x)[1];
                _runningAverage.at<Vec3w>(y,x)[2] += frame.at<Vec3b>(y,x)[2];
            }
        }
    }
    else
    {
        for(int y = 0; y < frame.rows; y++) {
            for(int x = 0; x < frame.cols; x++) {
                if(_pos > _runningAverageSize)
                    _runningAverage.at<ushort>(y,x) -= _runningAverage.at<ushort>(y,x) / _runningAverageSize;
                _runningAverage.at<ushort>(y,x) += frame.at<uchar>(y,x);
            }
        }
    }
}

void ForegroundExtractor::updateMovementHisotryImage(Mat &frame)
{
}

void ForegroundExtractor::updateMovementDensityImage(Mat &frame)
{
    //    assert((frame.type() == CV_8UC1));
    //    assert((_movementDensityImage.cols == frame.cols) && (_movementDensityImage.rows == frame.rows));

    Mat cop;
    threshold(_difference,cop,_movementDensityThreshold,1,CV_THRESH_BINARY);
    for(int y = 0; y < _rows; y++) {
        for(int x = 0; x < _cols; x++) {
            if(cop.at<uchar>(y,x)) {
                unsigned int i = (unsigned int)cop.at<uchar>(y,x) << (_pos % _movementDensitySize);
                _movementDensityImage.at<unsigned int>(y,x) = _movementDensityImage.at<unsigned int>(y,x) | i;
            }
            else {
                unsigned int i = (unsigned int)cop.at<uchar>(y,x) << (_pos % _movementDensitySize);
                _movementDensityImage.at<unsigned int>(y,x) = _movementDensityImage.at<unsigned int>(y,x) & !i;
            }
        }
    }
}

void ForegroundExtractor::delta(Mat &output) const
{
    _difference.copyTo(output);
}

void ForegroundExtractor::runningAverage(Mat &output) const
{
    float _max;
    if(!_greyscale) {
        //Compute the maximum value
        for(int y = 0; y < _runningAverage.rows; y++) {
            for(int x = 0; x < _runningAverage.cols; x++) {
                _max = (_max >= _runningAverage.at<Vec3w>(y,x)[0]) ? _max : _runningAverage.at<Vec3w>(y,x)[0];
                _max = (_max >= _runningAverage.at<Vec3w>(y,x)[1]) ? _max : _runningAverage.at<Vec3w>(y,x)[1];
                _max = (_max >= _runningAverage.at<Vec3w>(y,x)[2]) ? _max : _runningAverage.at<Vec3w>(y,x)[2];
            }
        }
        //Create output if necessary
        if(!((output.type() == CV_8UC3)
             && (output.cols == _runningAverage.cols)
             && (output.rows == _runningAverage.rows)))
            output.create(_runningAverage.rows,_runningAverage.cols,CV_8UC3);
        //Comput the output frame
        if(_pos > _runningAverageSize) {
            for(int y = 0; y < _runningAverage.rows; y++) {
                for(int x = 0; x < _runningAverage.cols; x++) {
                    output.at<Vec3b>(y,x)[0] = saturate_cast<uchar>( (float)_runningAverage.at<Vec3w>(y,x)[0] / _runningAverageSize );
                    output.at<Vec3b>(y,x)[1] = saturate_cast<uchar>( (float)_runningAverage.at<Vec3w>(y,x)[1] / _runningAverageSize );
                    output.at<Vec3b>(y,x)[2] = saturate_cast<uchar>( (float)_runningAverage.at<Vec3w>(y,x)[2] / _runningAverageSize );
                }
            }
        }
        else {
            for(int y = 0; y < _runningAverage.rows; y++) {
                for(int x = 0; x < _runningAverage.cols; x++) {
                    output.at<Vec3b>(y,x)[0] = saturate_cast<uchar>( (float)_runningAverage.at<Vec3w>(y,x)[0] / _pos );
                    output.at<Vec3b>(y,x)[1] = saturate_cast<uchar>( (float)_runningAverage.at<Vec3w>(y,x)[1] / _pos );
                    output.at<Vec3b>(y,x)[2] = saturate_cast<uchar>( (float)_runningAverage.at<Vec3w>(y,x)[2] / _pos );
                }
            }
        }
    }
    else {
        _max = max<ushort,1>(_runningAverage);

        //Create output if necessary
        if(!((output.type() == CV_8UC1)
             && (output.cols == _runningAverage.cols)
             && (output.rows == _runningAverage.rows)))
            output.create(_runningAverage.rows,_runningAverage.cols,CV_8UC1);
        //Comput the output frame
        if(_pos > _runningAverageSize)
            for(int y = 0; y < _runningAverage.rows; y++)
                for(int x = 0; x < _runningAverage.cols; x++)
                    output.at<uchar>(y,x) = saturate_cast<uchar>( (float)_runningAverage.at<ushort>(y,x) / _runningAverageSize );
        else
            for(int y = 0; y < _runningAverage.rows; y++)
                for(int x = 0; x < _runningAverage.cols; x++)
                    output.at<uchar>(y,x) = saturate_cast<uchar>( ((float)_runningAverage.at<ushort>(y,x)) / _pos);
    }
}

void ForegroundExtractor::movementHistoryImage(Mat &output) const
{

}

void ForegroundExtractor::movementDensityImage(Mat &output) const
{
//    //Create output if necessary
//    if(!((output.type() == CV_8UC1)
//         && (output.cols == _movementDensityImage.cols)
//         && (output.rows == _movementDensityImage.rows)))
//        output.create(_movementDensityImage.rows,_movementDensityImage.cols,CV_8UC1);
    //Comput the output frame
    for(int y = 0; y < _movementDensityImage.rows; y++) {
        for(int x = 0; x < _movementDensityImage.cols; x++) {
            unsigned int val = 0;
            unsigned int pix = _movementDensityImage.at<unsigned int>(y,x);
            for(unsigned int i = 0; i < _movementDensitySize; i++)
                if((pix & (0x1 << i)) == (unsigned int)(0x1 << i))
                    val += 1;
            output.at<uchar>(y,x) = saturate_cast<uchar>((double)val * 255.0 / (double)_movementDensitySize);
        }
    }
    threshold(output,output,30,255,CV_THRESH_BINARY);
}

void ForegroundExtractor::update()
{
    Mat frame;
    _capture >> frame;
    if(_blur)
        GaussianBlur(frame,frame,Size(7,7),3.0,3.0);
    if(_greyscale)
        cvtColor(frame,frame,CV_BGR2GRAY);
    //average
    updateRunningAverage(frame);
    //delta
    updateDelta(frame);
    //movement density image
    updateMovementDensityImage(frame);
    //movement history image
    updateMovementHisotryImage(frame);

    frame.copyTo(_source);
    _pos++;

}

void ForegroundExtractor::learn()
{
    clear();
    for(int i = 0; i < _referenceSize; i++)
    {
        update();
        usleep(15000);
    }
    runningAverage(_reference);
}

void ForegroundExtractor::clear()
{
}

void ForegroundExtractor::setBlur(bool value)
{
    _blur = value;
}

void ForegroundExtractor::setThreshold(bool value)
{
    _threshold = value;
}

void ForegroundExtractor::setThresholdValue(int value)
{
    _resultThreshold = value;
}

void ForegroundExtractor::result(Mat &output) const
{
    switch (_displayType) {
    case SOURCE_DISPLAY_TYPE:
        _source.copyTo(output);
        switch(_postTraitement)
        {
        case SOBEL_POST_TRAITEMENT:
            sobel(output,1,10);
            break;
        case LAPLACE_POST_TRAITEMENT:
            laplacian(output);
            break;
        case NORMALMAP_POST_TRAITEMENT:
            normalMap(output,1.0);
            break;
        }
        break;
    case LEARNED_IMAGE_DISPLAY_TYPE:
        _reference.copyTo(output);
        break;
    case DELTA_IMAGE_DISPLAY_TYPE:
        _difference.copyTo(output);
        break;
    case AVERAGE_IMAGE_DISPLAY_TYPE:
        runningAverage(output);
        break;
    case MHI_DISPLAY_TYPE:
        movementHistoryImage(output);
        break;
    case MDI_DISPLAY_TYPE:
        movementDensityImage(output);
        break;
    }
    if(_threshold)
        threshold(output,output,_resultThreshold,255,CV_THRESH_BINARY);
}

QImage ForegroundExtractor::getFrame()
{
    result(_result);
    return  mat2QImage(_result);
}

void ForegroundExtractor::sobel(Mat &value, int sigma, int threshold) const
{
    Mat sX;
    Mat sY;
    Mat kernX = (Mat_<char>(3,3) <<
                 -1,  0 , 1,
                 -2 * sigma,  0,  2 * sigma,
                 -1, -0 , 1);
    Mat kernY = (Mat_<char>(3,3) <<
                 1,  2 * sigma,  1,
                 0,  0,  0,
                 -1, -2 * sigma, -1);
    cvtColor(value,sX,CV_BGR2GRAY);
    cvtColor(value,sY,CV_BGR2GRAY);
    filter2D(sX,sX,0,kernX,Point(-1,-1),127);
    filter2D(sY,sY,0,kernY,Point(-1,-1),127);
    cvtColor(value,value,CV_BGR2GRAY);
    for( int y = 0; y < value.rows; y++ )
        for( int x = 0; x < value.cols; x++ )
            value.at<uchar>(y,x) = saturate_cast<uchar>( sqrt( (sX.at<uchar>(y,x) - 127) * (sX.at<uchar>(y,x) - 127)
                                                               + (sY.at<uchar>(y,x) - 127) * (sY.at<uchar>(y,x) - 127) ) );
    cv::threshold(value,value,threshold,0,CV_THRESH_TOZERO);
}


void ForegroundExtractor::normalMap(Mat &value, double scale) const
{
    static Mat sX = Mat::zeros( value.size(), CV_8U );
    static Mat sY = Mat::zeros( value.size(), CV_8U );
    Mat kernX = (Mat_<char>(3,3) <<
                 -1,  0 , 1,
                 -2,  0,  2,
                 -1, -0 , 1);
    Mat kernY = (Mat_<char>(3,3) <<
                 1,  2,  1,
                 0,  0,  0,
                 -1, -2, -1);
    cvtColor(value,sX,CV_BGR2GRAY);
    cvtColor(value,sY,CV_BGR2GRAY);
    filter2D(sX,sX,0,kernX,Point(-1,-1),127);
    filter2D(sY,sY,0,kernY,Point(-1,-1),127);
    for( int y = 0; y < value.rows; y++ ) {
        for( int x = 0; x < value.cols; x++ ) {
            value.at<Vec3b>(y,x)[0] = 255;
            value.at<Vec3b>(y,x)[1] = saturate_cast<uchar>(sY.at<uchar>(y,x));
            value.at<Vec3b>(y,x)[2] = saturate_cast<uchar>(sX.at<uchar>(y,x));
        }
    }
}

void ForegroundExtractor::laplacian(Mat &value) const
{
    Mat kern = (Mat_<char>(3,3) <<
                0, 1 , 0,
                1,  -4, 1,
                0, 1 , 0);
    cvtColor(value,value,CV_BGR2GRAY);
    GaussianBlur(value,value,Size(3,3),1.5,1.5);
    filter2D(value,value,0,kern);
}


//    assert(_source.data != frame.data);
//    assert((_source.type() == CV_8UC1) || (_source.type() == CV_8UC3));
//    assert(_source.type() == frame.type());
//    assert((_source.cols == frame.cols) && (_source.rows == frame.rows));

//void ForegroundExtractor::difference(const Mat &reference, const Mat &frame, Mat &output) const
//{
//    assert(reference.data != frame.data);
//    assert((reference.type() == CV_8UC1) || (reference.type() == CV_8UC3));
//    assert(reference.type() == frame.type());
//    assert((reference.cols == frame.cols) && (reference.rows == frame.rows));
//    //Create output if necessary
//    if(!((output.type() == CV_8UC1)
//         && (output.cols == frame.cols)
//         && (output.rows == frame.rows)))
//        output.create(frame.rows,frame.cols,CV_8UC1);
//    if(reference.type() == CV_8UC1)
//        for(int y = 0; y < reference.rows; y++)
//            for(int x = 0; x < reference.cols; x++)
//                output.at<uchar>(y,x) = saturate_cast<uchar>(abs(reference.at<uchar>(y,x) - frame.at<uchar>(y,x)));
//    else
//        for(int y = 0; y < reference.rows; y++)
//            for(int x = 0; x < reference.cols; x++) {
//                output.at<uchar>(y,x) = saturate_cast<uchar>(abs(
//                                                                 ((reference.at<Vec3b>(y,x)[0] + reference.at<Vec3b>(y,x)[1] + reference.at<Vec3b>(y,x)[2]))
//                        - ((frame.at<Vec3b>(y,x)[0] + frame.at<Vec3b>(y,x)[1] + frame.at<Vec3b>(y,x)[2]))
//                        )/3);
//            }
//}

//void ForegroundExtractor::addDifferenceFrame(const Mat &frame)
//{
//    assert((frame.type() == CV_8UC1));
//    assert((_MHI.cols == frame.cols) && (_MHI.rows == frame.rows));
//    Mat cop;
//    threshold(frame,cop,_threshold,1,CV_THRESH_BINARY);
//    for(int y = 0; y < frame.rows; y++) {
//        for(int x = 0; x < frame.cols; x++) {
//            if(cop.at<uchar>(y,x)) {
//                unsigned int i = (unsigned int)cop.at<uchar>(y,x) << (_pos % 32);
//                _MHI.at<unsigned int>(y,x) = _MHI.at<unsigned int>(y,x) | i;
//            }
//            else {
//                unsigned int i = (unsigned int)cop.at<uchar>(y,x) << (_pos % 32);
//                _MHI.at<unsigned int>(y,x) = _MHI.at<unsigned int>(y,x) & !i;
//            }
//        }
//    }
//    _pos++;
//}

//void ForegroundExtractor::differenceRunningAverage(Mat &output) const
//{
//    //Create output if necessary
//    if(!((output.type() == CV_8UC1)
//         && (output.cols == _MHI.cols)
//         && (output.rows == _MHI.rows)))
//        output.create(_MHI.rows,_MHI.cols,CV_8UC1);
//    //Comput the output frame
//    for(int y = 0; y < _MHI.rows; y++) {
//        for(int x = 0; x < _MHI.cols; x++) {
//            unsigned int val = 0;
//            unsigned int pix = _MHI.at<unsigned int>(y,x);
//            for(unsigned int i = 0; i < 32; i++)
//                if((pix & (0x1 << i)) == (unsigned int)(0x1 << i))
//                    val += 1;
//            output.at<uchar>(y,x) = saturate_cast<uchar>((double)val * 255.0 / 32.0);
//        }
//    }
//}

//void ForegroundExtractor::runningAverageImage(Mat &output) const
//{
//    float _max;
//    if(_color) {
//        //Compute the maximum value
//        for(int y = 0; y < _RAI.rows; y++) {
//            for(int x = 0; x < _RAI.cols; x++) {
//                _max = (_max >= _RAI.at<Vec3w>(y,x)[0]) ? _max : _RAI.at<Vec3w>(y,x)[0];
//                _max = (_max >= _RAI.at<Vec3w>(y,x)[1]) ? _max : _RAI.at<Vec3w>(y,x)[1];
//                _max = (_max >= _RAI.at<Vec3w>(y,x)[2]) ? _max : _RAI.at<Vec3w>(y,x)[2];
//            }
//        }
//        //Create output if necessary
//        if(!((output.type() == CV_8UC3)
//             && (output.cols == _RAI.cols)
//             && (output.rows == _RAI.rows)))
//            output.create(_RAI.rows,_RAI.cols,CV_8UC3);
//        //Comput the output frame
//        if(_pos > _averageWindowsSize) {
//            for(int y = 0; y < _RAI.rows; y++) {
//                for(int x = 0; x < _RAI.cols; x++) {
//                    output.at<Vec3b>(y,x)[0] = saturate_cast<uchar>( (float)_RAI.at<Vec3w>(y,x)[0] / _averageWindowsSize );
//                    output.at<Vec3b>(y,x)[1] = saturate_cast<uchar>( (float)_RAI.at<Vec3w>(y,x)[1] / _averageWindowsSize );
//                    output.at<Vec3b>(y,x)[2] = saturate_cast<uchar>( (float)_RAI.at<Vec3w>(y,x)[2] / _averageWindowsSize );
//                }
//            }
//        }
//        else {
//            for(int y = 0; y < _RAI.rows; y++) {
//                for(int x = 0; x < _RAI.cols; x++) {
//                    output.at<Vec3b>(y,x)[0] = saturate_cast<uchar>( (float)_RAI.at<Vec3w>(y,x)[0] / _pos );
//                    output.at<Vec3b>(y,x)[1] = saturate_cast<uchar>( (float)_RAI.at<Vec3w>(y,x)[1] / _pos );
//                    output.at<Vec3b>(y,x)[2] = saturate_cast<uchar>( (float)_RAI.at<Vec3w>(y,x)[2] / _pos );
//                }
//            }
//        }
//    }
//    else {
//        //Compute the maximum value
//        //        for(int y = 0; y < _RAI.rows; y++)
//        //            for(int x = 0; x < _RAI.cols; x++)
//        //                max = (max >= _RAI.at<ushort>(y,x)) ? max : _RAI.at<ushort>(y,x);
//        _max = max<ushort,1>(_RAI);

//        //Create output if necessary
//        if(!((output.type() == CV_8UC1)
//             && (output.cols == _RAI.cols)
//             && (output.rows == _RAI.rows)))
//            output.create(_RAI.rows,_RAI.cols,CV_8UC1);
//        //Comput the output frame
//        if(_pos > _averageWindowsSize)
//            for(int y = 0; y < _RAI.rows; y++)
//                for(int x = 0; x < _RAI.cols; x++)
//                    output.at<uchar>(y,x) = saturate_cast<uchar>( (float)_RAI.at<ushort>(y,x) / _averageWindowsSize );
//        else
//            for(int y = 0; y < _RAI.rows; y++)
//                for(int x = 0; x < _RAI.cols; x++)
//                    output.at<uchar>(y,x) = saturate_cast<uchar>( ((float)_RAI.at<ushort>(y,x)) / _pos);
//    }
//}

